#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>

// 封装的Harris关键点检测函数
pcl::PointCloud<pcl::PointXYZI>::Ptr detectHarrisKeypoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& input_cloud)
{
    // 计算法线
    pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;
    ne.setInputCloud(input_cloud);
    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>());
    ne.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(0.03);
    ne.compute(*cloud_normals);

    // 设置Harris关键点检测的参数
    pcl::HarrisKeypoint3D<pcl::PointXYZI, pcl::PointXYZI, pcl::Normal> harris;
    harris.setInputCloud(input_cloud);
    harris.setNormals(cloud_normals);
    harris.setNonMaxSupression(true); // 开启非极大值抑制
    harris.setRadius(0.01); // 调整半径
    harris.setThreshold(1e-5); // 调整阈值
    pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZI>);
    harris.compute(*keypoints);

    return keypoints;
}

// 封装的SIFT关键点检测函数
pcl::PointCloud<pcl::PointWithScale>::Ptr detectSIFTKeypoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& input_cloud)
{
    // 设置SIFT关键点检测的参数
    pcl::SIFTKeypoint<pcl::PointXYZI, pcl::PointWithScale> sift;
    pcl::PointCloud<pcl::PointWithScale>::Ptr keypoints(new pcl::PointCloud<pcl::PointWithScale>);
    sift.setInputCloud(input_cloud);
    sift.setScales(0.01f, 8, 1);
    sift.setMinimumContrast(0.0);
    sift.compute(*keypoints);

    return keypoints;
}

// 回调函数，用于处理接收到的点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, ros::Publisher& pub, ros::Publisher& voxel_pub, ros::Publisher& sor_pub, ros::Publisher& keypoints_pub)
{
    // 将ROS的点云消息转换为PCL的点云格式
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud_msg, *cloud);

    // 打印点云中的点数
    ROS_INFO("Received point cloud with %lu points", cloud->points.size());

    // 将二维点云扩展为三维点云（z坐标设置为0），并添加intensity字段
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_with_intensity(new pcl::PointCloud<pcl::PointXYZI>);
    for (const auto& point : cloud->points) {
        pcl::PointXYZI p;
        p.x = point.x;
        p.y = point.y;
        p.z = 0.0;
        p.intensity = 1.0; // 这里设置一个默认的intensity值，你可以根据需要进行修改
        cloud_with_intensity->points.push_back(p);
    }

    // 发布原始点云
    sensor_msgs::PointCloud2 original_output;
    pcl::toROSMsg(*cloud_with_intensity, original_output);
    original_output.header.frame_id = cloud_msg->header.frame_id;
    pub.publish(original_output);

    // 预处理点云（下采样和去除噪声）
    pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
    voxel_filter.setInputCloud(cloud_with_intensity);
    voxel_filter.setLeafSize(0.01f, 0.01f, 0.01f);
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    voxel_filter.filter(*voxel_filtered_cloud);

    // 发布下采样后的点云
    sensor_msgs::PointCloud2 voxel_output;
    pcl::toROSMsg(*voxel_filtered_cloud, voxel_output);
    voxel_output.header.frame_id = cloud_msg->header.frame_id;
    voxel_pub.publish(voxel_output);

    pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
    sor.setInputCloud(voxel_filtered_cloud);
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);
    pcl::PointCloud<pcl::PointXYZI>::Ptr sor_filtered_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    sor.filter(*sor_filtered_cloud);

    // 发布去除噪声后的点云
    sensor_msgs::PointCloud2 sor_output;
    pcl::toROSMsg(*sor_filtered_cloud, sor_output);
    sor_output.header.frame_id = cloud_msg->header.frame_id;
    sor_pub.publish(sor_output);

    // 调用Harris关键点检测函数
    // pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints = detectHarrisKeypoints(sor_filtered_cloud);

    // 调用SIFT关键点检测函数
    pcl::PointCloud<pcl::PointWithScale>::Ptr keypoints = detectSIFTKeypoints(sor_filtered_cloud);
    // 检查关键点是否为空
    if (keypoints->empty()) {
        ROS_INFO("No keypoints detected.");
        return;
    }
    
    // 将SIFT关键点转换为适合pcl::toROSMsg的格式
    pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints_xyz(new pcl::PointCloud<pcl::PointXYZI>);
    for (const auto& point : keypoints->points) {
        pcl::PointXYZI p;
        p.x = point.x;
        p.y = point.y;
        p.z = point.z;
        p.intensity = point.scale;
        keypoints_xyz->points.push_back(p);
    }

    // 发布关键点
    sensor_msgs::PointCloud2 keypoints_output;
    pcl::toROSMsg(*keypoints_xyz, keypoints_output);
    keypoints_output.header.frame_id = cloud_msg->header.frame_id;
    keypoints_pub.publish(keypoints_output);
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pcl_point_cloud_reader");
    ros::NodeHandle nh;

    // 创建发布器
    ros::Publisher original_pub = nh.advertise<sensor_msgs::PointCloud2>("original_cloud", 1);
    ros::Publisher voxel_pub = nh.advertise<sensor_msgs::PointCloud2>("voxel_filtered_cloud", 1);
    ros::Publisher sor_pub = nh.advertise<sensor_msgs::PointCloud2>("sor_filtered_cloud", 1);
    ros::Publisher keypoints_pub = nh.advertise<sensor_msgs::PointCloud2>("keypoints", 1);

    // 打开rosbag文件
    rosbag::Bag bag;
    bag.open("/home/zhangjj/dm_ws/src/function_test/bag/bed_scan.bag", rosbag::bagmode::Read);

    // 创建一个视图来读取bag文件中的消息
    rosbag::View view(bag, rosbag::TopicQuery("/scan_matched_points2"));

    // 播放bag文件中的消息
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::PointCloud2ConstPtr cloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
        if (cloud_msg != NULL)
        {
            pointCloudCallback(cloud_msg, original_pub, voxel_pub, sor_pub, keypoints_pub);
        }
        ros::Duration(0.1).sleep();
        // 检查是否有退出信号
        if (ros::isShuttingDown()) {
            ROS_INFO("收到退出信号，程序即将关闭。");
            break; // 退出for循环
        }
    }

    // 关闭bag文件
    bag.close();

    return 0;
}